#include "main.h"
#include "gpio.h"
#include "tim.h"
#include "motor.h"
#include "PID.h"

void motor_init(MOTOR_t *motor)
{
    motor->duty = 0;
    motor->overflow_num = 0;
    motor->count[0] = 0;
    motor->count[1] = 0;
    motor->position[0] = 0;
    motor->position[1] = 0;
    motor->speed = 0;
    
    PID_param_init(&(motor->pid_speed),0.3,0.15,0.15,100,0);//实测控速时不宜用输出死区


    __HAL_TIM_SetCounter(ENCODER_TIM,0);
    HAL_TIM_Base_Start(ENCODER_TIM);

    HAL_TIM_Encoder_Start(ENCODER_TIM,ENCODER_CHA);
    HAL_TIM_Encoder_Start(ENCODER_TIM,ENCODER_CHB);

    __HAL_TIM_SetCounter(PWM_TIM,0);
    HAL_TIM_Base_Start_IT(PWM_TIM);

    __HAL_TIM_SetCompare(PWM_TIM,PWM_CH,0);
    HAL_TIM_PWM_Start(PWM_TIM,PWM_CH);

}
void motor_config(uint8_t motor_state)
{
    if(motor_state == MOTOR_OFF)HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_RESET);
    else HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);
}
void motor_set_speed(MOTOR_t *motor,float rpm)
{
    //单位为rpm，顺时针为正
    motor->pid_speed.tar = rpm;
}

void motor_work(MOTOR_t *motor)
{
    //本函数每50ms工作一次
    //获取当前电机的位置和速度
    motor->count[0] = __HAL_TIM_GetCounter(ENCODER_TIM);
    ////获取TIM的更新次数
    if(motor->count[1] > 65000 && motor->count[0] < 1000)motor->overflow_num++;
    else if(motor->count[1] < 1000 && motor->count[0] > 65000)motor->overflow_num--;
    ////换算
    motor->position[0] = (motor->count[0]+motor->overflow_num*65535)/1560.0f;
    motor->speed = (motor->position[0]-motor->position[1])*1200;
    ////计数值传递
    motor->count[1] = motor->count[0];
    motor->position[1] = motor->position[0];
    //开始PID计算
    motor->pid_speed.act = motor->speed;
    PID_calculate(&(motor->pid_speed));
    
    //控制方向
    if(motor->pid_speed.out > 0)
    {
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET); 
        motor->duty = (uint8_t)motor->pid_speed.out;
    }
    else
    {
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
        motor->duty = (uint8_t)-motor->pid_speed.out;
    }
    __HAL_TIM_SetCompare(PWM_TIM,PWM_CH,motor->duty);//ARR为100
    
}